#include <Eigen/Dense>
#include <string>
#include <string.h>
// #include <sys/time.h>
// #include <ctime>
// #include <chrono>
// #include <iomanip>
// #include <stdio.h>
#include <iostream>

#include "global.h"
#include "params.h"
using namespace std;
using Eigen::Matrix3d;
using Eigen::Vector3d;

Matrix3d R(string axis, double angle) // 计算x,y,z欧拉旋转矩阵
{
  angle = angle * 3.1415926 / 180.0; // sin函数参数为弧度制，需转换
  Matrix3d mat;
  if (axis == "x") {
    mat << 1, 0, 0, 0, cos(angle), -sin(angle), 0, sin(angle), cos(angle);
  } else if (axis == "y") {
    mat << cos(angle), 0, sin(angle), 0, 1, 0, -sin(angle), 0, cos(angle);
  } else if (axis == "z") {
    mat << cos(angle), -sin(angle), 0, sin(angle), cos(angle), 0, 0, 0, 1;
  }
  return mat;
}

Matrix3d Antisymmetric(Vector3d vec) {
  Matrix3d mat;
  mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0;
  return mat;
}

Matrix3d JacobianMatrix(Vector3d vec, int ksi) {
  Matrix3d tmpmat;
  tmpmat(0, 0) = 0;
  tmpmat(0, 1) = -L_2 * cos(vec(1)) - L_3 * cos(vec(1) + vec(2));
  tmpmat(0, 2) = -L_3 * cos(vec(1) + vec(2));
  tmpmat(1, 0) = ksi * L_1 * sin(vec(0)) +
                 L_3 * cos(vec(0)) * cos(vec(1) + vec(2)) +
                 L_2 * cos(vec(1)) * cos(vec(0));
  tmpmat(1, 1) = -L_3 * sin(vec(0)) * sin(vec(1) + vec(2)) -
                 L_2 * sin(vec(1)) * sin(vec(0));
  tmpmat(1, 2) = -L_3 * sin(vec(0)) * sin(vec(1) + vec(2));
  tmpmat(2, 0) = ksi * L_1 * cos(vec(0)) +
                 L_3 * sin(vec(0)) * cos(vec(1) + vec(2)) +
                 L_2 * cos(vec(1)) * sin(vec(0));
  tmpmat(2, 1) = L_3 * cos(vec(0)) * sin(vec(1) + vec(2)) +
                 L_2 * sin(vec(1)) * cos(vec(0));
  tmpmat(2, 2) = L_3 * cos(vec(0)) * sin(vec(1) + vec(2));

  return tmpmat;
}

// #define time_print  //需要打印系统时间时保留

// /*
//     时间测量函数
//     用法：
//         #include <timetest.cpp>

//         float tStart = timetest();
//         //待测量代码段
//         float tEnd = timetest();
//         cout<<tEnd-tStart;
// */

// float timetest()
// {
//     auto now = std::chrono::system_clock::now();
//     // 通过不同精度获取相差的毫秒数
//     uint64_t dis_millseconds =
//     std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count()
//     -
//     std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count()
//     * 1000;
//     time_t tt = std::chrono::system_clock::to_time_t(now);
//     auto time_tm = localtime(&tt);
//     char strTime[25] = {0};
//     float ttt = time_tm->tm_hour * 3600 + time_tm->tm_min * 60 +
//     time_tm->tm_sec + 0.001*(float)dis_millseconds;
//     #ifdef time_print
//     sprintf(strTime, "%d-%02d-%02d %02d:%02d:%02d %03d", time_tm->tm_year +
//     1900, time_tm->tm_mon + 1, time_tm->tm_mday, time_tm->tm_hour,
//     time_tm->tm_min, time_tm->tm_sec, (int)dis_millseconds);
//     std::cout << strTime << std::endl;
//     #else
//     #endif

//     return ttt;
// }